/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdlib.h>
#include <string.h>

#include "command.h"

// extra 3 for cr, lf and \0
#define MAX_LINE 100
#define LINE_ARRAY_SIZE (MAX_LINE+3)
#define NEWLINE "\n"

Telemetry_t telemetry;

typedef struct {
  VehicleInfo_t info;
  SystemCharacter_t * systems [32];
} __attribute__ ((packed))Capabilities_t;

Capabilities_t capabilities;
Coordinate_t coordinate;
Custody_t custody;

#define TRUE 1
#define FALSE 0

uint8_t command[CMD_MAX_SIZE];
CommandQueue commandQueue;

int main(int argc, char *argv[])
{
  int sockfd, portno;
  struct sockaddr_in serv_addr;
  struct hostent *server;

  if(argc < 2) { printf("Usage: %s IP [TYPE]\n",argv[0]); exit(0);}
  char *server_IP=argv[1];
  portno = 12345;
  char pkt_type;
  if(argc > 2) pkt_type = argv[2][0];

  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
  if (sockfd < 0) {
    printf("ERROR: opening socket\n");
    exit(0);
  }
  server = gethostbyname(server_IP);
  if (server == NULL) {
    printf("ERROR: no such host\n");
    exit(0);
  }

  bzero((char *) &serv_addr, sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  bcopy((char *)server->h_addr, 
      (char *)&serv_addr.sin_addr.s_addr,
      server->h_length);
  serv_addr.sin_port = htons(portno);

  telemetry.gps.pos.lat=40.138;
  telemetry.gps.pos.lon=-105.244;
  telemetry.airdata.tas=30;
  telemetry.airdata.alt=1800;
  telemetry.system.availableForCommand=1;
  telemetry.system.twelveV=1190;
  telemetry.angles.roll=1234;
  telemetry.angles.pitch=2341;
  telemetry.angles.yaw=3412;
  telemetry.navigation.waypt=5;

  strcpy(capabilities.info.id,"Ares3");
  //capabilities.data = 0;
  //capabilities.flight_computer = 1;
  //capabilities.gps = 1;
  //capabilities.air_data = 1;
  //capabilities.health_and_status = 1;

  coordinate.partner.s_addr = inet_addr("6.1.77.50");
  coordinate.requesting = true;

  custody.parent.s_addr = inet_addr("6.1.83.49");
  custody.requesting = true;

  int n;

  switch(pkt_type) {
    case 't':
      CMD_CMD(command) = CMD_TELEM;
      CMD_NUM(command) = sizeof(telemetry);
      CopyStructToBuf(telemetry, &command[3]);
      CMD_CSUM(command) = C_calcCheckSum(command);
      break;
    case 'a':
      CMD_CMD(command) = CMD_ACK;
      CMD_NUM(command) = 0;
      CMD_CSUM(command) = C_calcCheckSum(command);
      break;
    case 'c':
      CMD_CMD(command) = CMD_COORDINATE;
      CMD_NUM(command) = sizeof(coordinate);
      CopyStructToBuf(coordinate, &command[3]);
      CMD_CSUM(command) = C_calcCheckSum(command);
      break;
    case 'u':
      CMD_CMD(command) = CMD_CUSTODY;
      CMD_NUM(command) = sizeof(custody);
      CopyStructToBuf(custody, &command[3]);
      CMD_CSUM(command) = C_calcCheckSum(command);
      break;
    default:
      CMD_CMD(command) = CMD_CAPABILITY;
      //CMD_NUM(command) = sizeof(capabilities);
      CMD_NUM(command) = 32;
      //CopyStructToBuf(capabilities, &command[3]);
      strcpy((char *)&command[3], capabilities.info.id);
      CMD_CSUM(command) = C_calcCheckSum(command);
      break;
  }

  n = sendto(sockfd, command, CMD_SIZE(command), 0, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
  printf("Sent %d bytes\n",n);
  if (n < 0) {
    printf("ERROR: writing to socket\n");
    exit(0);
  }
  return 0;
}
